﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using TrackingSim.Filtering;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Media;
using TrackingSim.Filtering.Tracks;
using TrackingSim.Filtering.Kinematics;

namespace TrackingSim.GraphicSystem
{
    class Drawing
    {
        private static double sqr(double x)
        {
            return x * x;
        }

        private static MathLib.Vector[] get_ellipse(int step_count, double major_axis_length, double minor_axis_length)
        {
            MathLib.Vector[] ellipse = new MathLib.Vector[2 * step_count];

            for (int i = 0; i < step_count; ++i)
            {
                ellipse[i] = new MathLib.Vector(2);
                ellipse[i + step_count] = new MathLib.Vector(2);

                double percent_through_axis = (double)i / (double)step_count;
                double x = percent_through_axis * (2 * major_axis_length) - major_axis_length;

                double y_plus = Math.Sqrt(sqr(minor_axis_length) - (sqr(minor_axis_length) / sqr(major_axis_length) * sqr(x)));
                double y_minus = -y_plus;

                ellipse[i].X = x;
                ellipse[i].Y = y_plus;
                ellipse[i + step_count].X = x;
                ellipse[i + step_count].Y = y_minus;
            }

            return ellipse;
        }

        public static VertexPositionNormalTexture[] get_ellipse_vertices(Track track, int step_count, ref int vertex_count)
        {
            State_R6 state = (State_R6)track.KS;

            double x_axis_length = Math.Sqrt(state.Cov.m[0, 0]);
            double y_axis_length = Math.Sqrt(state.Cov.m[1, 1]);
            double z_axis_length = Math.Sqrt(state.Cov.m[2, 2]);

            VertexPositionNormalTexture[] ellipse_xy = new VertexPositionNormalTexture[2 * step_count];
            MathLib.Vector[] vectors_xy = get_ellipse(step_count, x_axis_length, y_axis_length);

            for (int i = 0; i < step_count; ++i)
            {
                ellipse_xy[i].Position.X = (float)state.X + (float)vectors_xy[i].X;
                ellipse_xy[i].Position.Y = (float)state.Y + (float)vectors_xy[i].Y;
                ellipse_xy[i].Position.Z = (float)state.Z;

                ellipse_xy[i + step_count].Position.X = (float)state.X + (float)vectors_xy[i + step_count].X;
                ellipse_xy[i + step_count].Position.Y = (float)state.Y + (float)vectors_xy[i + step_count].Y;
                ellipse_xy[i + step_count].Position.Z = (float)state.Z;
            }

            VertexPositionNormalTexture[] ellipse_xz = new VertexPositionNormalTexture[2 * step_count];
            MathLib.Vector[] vectors_xz = get_ellipse(step_count, x_axis_length, z_axis_length);

            for (int i = 0; i < step_count; ++i)
            {
                ellipse_xz[i].Position.X = (float)state.X + (float)vectors_xy[i].X;
                ellipse_xz[i].Position.Y = (float)state.Y;
                ellipse_xz[i].Position.Z = (float)state.Z + (float)vectors_xy[i].Y;

                ellipse_xz[i + step_count].Position.X = (float)state.X + (float)vectors_xy[i + step_count].X;
                ellipse_xz[i + step_count].Position.Y = (float)state.Y;
                ellipse_xz[i + step_count].Position.Z = (float)state.Z + (float)vectors_xy[i + step_count].Y;
            }

            VertexPositionNormalTexture[] ellipse_yz = new VertexPositionNormalTexture[2 * step_count];
            MathLib.Vector[] vectors_yz = get_ellipse(step_count, y_axis_length, z_axis_length);

            for (int i = 0; i < step_count; ++i)
            {
                ellipse_yz[i].Position.X = (float)state.X;
                ellipse_yz[i].Position.Y = (float)state.Y + (float)vectors_xy[i].X;
                ellipse_yz[i].Position.Z = (float)state.Z + (float)vectors_xy[i].Y;

                ellipse_yz[i + step_count].Position.X = (float)state.X;
                ellipse_yz[i + step_count].Position.Y = (float)state.Y + (float)vectors_xy[i + step_count].X;
                ellipse_yz[i + step_count].Position.Z = (float)state.Z + (float)vectors_xy[i + step_count].Y;
            }

            VertexPositionNormalTexture[] ellipse = new VertexPositionNormalTexture[6 * step_count];
            ellipse_xy.CopyTo(ellipse, 0);
            ellipse_xz.CopyTo(ellipse, 2 * step_count);
            ellipse_yz.CopyTo(ellipse, 4 * step_count);

            vertex_count = 6 * step_count;
            return ellipse;
        }

        private List<VertexPositionColor> GetTriangleStrip(Vector3[] points, float thickness, ref int triangleCount)
        {
            triangleCount = 0;

            Vector3 lastPoint = Vector3.Zero;
            List<VertexPositionColor> list = new List<VertexPositionColor>();
            for (int i=0;i<points.Length;i++)
            {
                if (i == 0) { lastPoint = points[i]; continue; }
                //the direction of the current line
                Vector3 direction = lastPoint - points[i];
                direction.Normalize();
                //the perpendiculat to the current line
                Vector3 normal = Vector3.Cross(direction, Vector3.UnitZ);
                normal.Normalize();
                Vector3 p1 = lastPoint + normal * thickness; triangleCount++;
                Vector3 p2 = lastPoint - normal * thickness; triangleCount++;
                Vector3 p3 = points[i] + normal * thickness; triangleCount++;
                Vector3 p4 = points[i] - normal * thickness; triangleCount++;
                list.Add(new VertexPositionColor(p1, Color.Black));
                list.Add(new VertexPositionColor(p2, Color.Black));
                list.Add(new VertexPositionColor(p3, Color.Black));
                list.Add(new VertexPositionColor(p4, Color.Black));
                lastPoint = points[i];
            }
            return list;
        }
    }
}
